/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is a variant of Branch and Bound localizer. This uses point cloud as a map and matches 
* using Aproximative nearest neighbour search. The interface and many of the variables 
* are kept same (for now) as with the predesessor...
* 
* Please note that there is now all occupancy related code, but it is not used.
* 
* @author Jari Saarinen
*/

#ifndef _B_ET_B_ANN_LOCALIZER_H_
#define _B_ET_B_ANN_LOCALIZER_H_

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <math.h>
#include <time.h>
#include "MatchStorage.h" ///<Output "definition"
#include "EnvironmentMeasurement2D.h" ///<Input "definition"
#include "TOccupancyGrid.h"
#include <ANN/ANN.h>


#define		MAXNBOFDATAPNTSFORMAPREGCOMPUT	5000
#define		MAXNBOFDATAPNTCELLS				1000000

#ifndef PI
#define PI 3.1415926536
#endif
 
typedef struct TwoDPointStruct {
		/* Surface plane parameters */
		float	x;
		float	y;
} TwoDPoint;

typedef struct RegistrationPnt {
		TwoDPoint		*dataPnts;
		int			nbOfRegDataPnts;
		float		filteredDataPntDistanceFromMapCenter;
} RegistrationPntData;


typedef struct CellAlignmentSpanStruct {
		float				dx;
		float				dy;
		float				dphii;
} CellAlignmentSpan;

typedef struct CellAlignmentCentreStruct {
		float				x;
		float				y;
		float				phii;
} CellAlignment;

typedef struct CellAlignmentStruct {
		CellAlignmentSpan	cellAlignHalfSpan;
		CellAlignment		cellAlignCentre;
		float				upperBoundForCellScore;
		float				cellCenterScore;
} CellAlignmentData;


class CBetBANNLocalizer{
		public:
				/**
				* Constructor for line map input
				* @param *mapfile path to line map file
				* @param resolution The requested resolution for occupancy grid
				*/
				CBetBANNLocalizer(const char *mapfile,float resolution);	
				
				/**
				* Constructor for occu map input
				* @param *mapfile path to occu map file
				*/
				CBetBANNLocalizer(const char *mapfile);	
				
				/**
				 * Constructor for empty map = SLAM version
				 * @param size the size of the map in meters
				 * @param resolution The requested resolution for occupancy grid
				 */
				CBetBANNLocalizer(float map_size,float resolution);	
				
				void intializeANN();
				
				/**
				* Destructor
				*/
				~CBetBANNLocalizer();
				/**
				* Localizator interface
				* @param &meas [in] reference to CEnvironmentMeasurement2D class 
				* @param &out [out] a reference to CMatchStorage, which is filled according to result and nSolutions
				* @return true, if all was ok.
				*/
				bool localize(CEnvironmentMeasurement2D &meas, CMatchStorage &out);
				
				/**
				* Update the environment measurement to the map
				* Transforms the data to reference frame and recomputes the distance transform
				* @param &meas [in] reference to CEnvironmentMeasurement2D class 
				**/
				void setMeasurementToMap(CEnvironmentMeasurement2D &meas);
				/**
				 * Update the environment measurement to the map
				 * Transforms the data to reference frame and recomputes the distance transform
				 * @param &meas [in] reference to Grid::TScan 
				 * @param x,y,a pose where the scan was taken 
				 * @param P probability used in update (1-P) is empty and P is occupied
				 * @param updateDist Set true to update the internal distance transform
				 * @param max_laser_dist If set to grater that zero, then the occupancy grid method, which 
				*                        updates the empty are based on laser range values exceding the 
				*                        value @max_laser_dist is used 
				 **/
				void setMeasurementToMap(Grid::TScan &meas, float x,float y, float a, float P,bool updateDist=true, float max_laser_dist=-1);
				
				/**
				 * Update the environment measurement to the map
				 * Transforms the data to reference frame and recomputes the distance transform
				 * @param &meas [in] reference to Grid::TScan 
				 * @param x,y,a pose where the scan was taken 
				 **/
				void setMeasurementToMap(Grid::TScan &meas, float x,float y, float a);
				
				/**
				 * Sets the tuning parameters for algorithm
				 * @param fdist big number will cause more angles to be viewed
				 * @param th small number will cause more accurate search
				 * defaults: paramTh=0.6; paramFdist=4.0;
				 **/
				void tuneParameters(float fdist_,float th);
				void updateDistTransform();
				
				/**
				* Save the grid map to specified file
				*/
				void save(const char *filename){grid->save(filename);};
				/**
				* Load the gridmap from the file
				*/
				bool load(const char *filename){return grid->load(filename);}
				
				void clearMap(){
					GlobalMap.clear();	
				}		

				TOccupancyGrid *grid;
				float *distanceTransform;
		private:
				CEnvironmentMeasurement2D GlobalMap; ///< points in the reference frame
				float maxdist; ///< the maximum distance in distance transform
				float map_resolution;
				int Nx; ///< Number of pixels in x-dir
				int Ny; ///< Number of pixels in y-dir
				float paramTh;
				float paramFdist;
				float paramMinSpan;
				float paramISpan;
				int paramMinReguiredRegDataPntNb;
				bool paramIsMapinit;
				
				float 		*pb_cost;
				unsigned int	*pb_ind;
								
				CellAlignmentData *cellAlignmentData; ///< matchers internal data structure
				
				///ANN data
				double		eps;			// error bound
				int				maxPts;			// maximum number of data points (more will segfault!!)
				ANNpointArray		dataPts;				// data points
				ANNpoint			queryPt;				// query point
				ANNidxArray			nnIdx;					// near neighbor indices
				ANNdistArray		dists;					// near neighbor distances
				ANNkd_tree*			kdTree;					// search structure
				
				/**
				* Internal initializer
				*/
				bool initialize();
				
				
				int	computeShortestDistanceBtwLocalMapElementWrtElementsInGlobalMap(
																												TwoDPoint				pntInTheLocalMap,
																												float					cosPhii,
																												float					sinPhii,
																												float*	globalMap,
																												TOccupancyGrid *O,
																												float					deltaC,
																												TwoDPoint			currentAlignment,
																												float*					distance,
																												float*					best,
																												float 					pTh);
				
				int	computeDistanceToBeAddedToComputeUpperBoundForASingleElementScore(
																												TwoDPoint			pntInTheLocalMap,
																												float				deltaXYZC,
																												float				sinPhiiPer2,
																												float				cosPhii,
																												float				sinPhii,
																												CellAlignmentSpan	cellAlignHalfSpan,
																												float*				deltaC );
				
				int	calc_best_worst( 
														RegistrationPntData*	localMap,
														float*	r,
														float*	globalMap,
														TOccupancyGrid *O,
														CellAlignmentData*		cellAlignmentData,
														float*					upperBoundAvgLikelihood,
														float*					Likelihood,
														float					pTh );
				CellAlignmentData*	divideEachCellInTheCurrentLevelIntoTwoSubCells( 
																										CellAlignmentData*	cellAlignmentData,
																										unsigned int index,
																										unsigned int		*indexLess,
																										unsigned int		*indexMore,
																										unsigned int		*nbOfCells,
																										float	filteredDataPntDistanceWithinTheLocalMap,
																										float	*maxSpan);
				void up_heap(float *heap,unsigned int *indi, float val, unsigned int ind,unsigned int *N);
				void down_heap(float *heap,unsigned int *indi, float *val,unsigned  int *ind, unsigned int *N);
				
				int  BetBloc(unsigned int *pb_ind,
										float *pb_cost,
										CellAlignmentData* 	cellAlignmentData,
										TOccupancyGrid *O,
										float*	globalMap,
										RegistrationPntData*	localMap,
										float *r
										//float filteredDataPntDistanceWithinTheLocalMap
										//float paramTh
								);
				
				int	viewPointRegistration(CellAlignmentData *cellAlignmentData, 
																	TOccupancyGrid *O,
																	float *globalMap,
																	RegistrationPntData *localMap,
																	float *r);
																	//float paramTh, 
																	//float paramFdist);
				
				/**
				* Calculates the number of points in the trajectory that is closer than @dTh to an obstacle
				* @param *cellAlignmentData The cell alligment data structure
				* @param dTh Distance threshold to an obstacle
				*/ 
				int calculateTrajectoryScore(CellAlignmentData* cellAlignmentData,float dTh);
				
				/**
				 * Calculate upper bound (the best possible) score and "worst" score for 
				 * Trajectory. The smaller the value the better
				 * @param *cellAlignmentData[in] The cell alligment data 
				 * @param pTh[in] The threshold for determining if path is impossible(The trajectory 
														is allowed to pass 2xpTh from the obstacle); 
				 * @param &upperBound[out] upper bound estimate
				 * @param &centerScore[out] The cell center score for trajectory 
				 * @return 0
				 */
				int	calc_traj_best_worst(CellAlignmentData*		cellAlignmentData,
																																				float	pTh,
																																				float &upperBound,
																																				float &centerScore);
																																				
				

		
};



#endif